package org.team751.subsystems;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.team751.RobotMap;
import org.team751.util.RelaySpeedController;
import org.team751.sensors.ShooterPotentiometer;

/**
 * This subsystem manages the tread-based angle adjusting mechanism of the shooter
 */
public class ShooterTread extends Subsystem {
   private RelaySpeedController relay = new RelaySpeedController(RobotMap.shooterRollersChannel);
   private ShooterPotentiometer potentiometer = new ShooterPotentiometer(RobotMap.shooterPotentiometer);

    //PID constants for the angle one: Need tuning
   /**
    * The P constant
    */
   public static final double kPAngle = 0.3;
   /**
    * The I constant
    */
   public static final double kIAngle = 0;
    /**
     * The D constant
     */
    public static final double kDAngle = 0;


    //This is about 1.25 inches, but we should measure it once it's made.
    /**
     *
     */
    public static final double ROLLER_DIAMETER = 1.25;
    //Should double-check this once it's made as well.
    /**
     *
     */
    public static final double SHOOTER_RADIUS = 10.5;

    private PIDController anglePID = new PIDController(kPAngle,kIAngle,kDAngle, potentiometer, relay);

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }

    /**
     *
     * @param angleGoal
     */
    public void targetAngle(double angleGoal) {
        anglePID.setSetpoint(angleGoal);
    }
}